import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image, PointCloud2
from message_filters import ApproximateTimeSynchronizer, Subscriber

class ObjectTrackingSubscriber(Node):

    def __init__(self):
        super(ObjectTrackingSubscriber, self).__init__("ng_object_trackinode")

        #Hard code the topics which should be subscribed
        self.image_sub = Subscriber(self, Image, "/camera/color/image_raw")
        self.plc_sub = Subscriber(
            self, PointCloud2, "/camera/depth/points", qos_profile=qos_profile_sensor_data
        )
        queue_size = 30
        self.ts = ApproximateTimeSynchronizer(
            [self.image_sub, self.plc_sub],
            queue_size,
            0.01
        )
        self.ts.registerCallback(self.object_tracking_CB)
        self.is_first = True

    # TODO: develop your ground break tracking algorithm here, hhh
    def object_tracking_CB(self, img_msg: Image, pcl_msg: PointCloud2):
        print(img_msg.header.stamp, pcl_msg.header.stamp)

        if self.is_first:
            self.is_first = False

        pass

def main(args=None):
    rclpy.init(args=args)
    test_node = ObjectTrackingSubscriber()
    rclpy.spin(test_node)

    test_node.destroy_node()
    rclpy.shutdown()



if __name__ == "__main__":
    main()